#include "../Device/control.h"

#include "../Device/maintask.h"
#include "cmsis_os2.h"
#include "main.h"

#define FRICTION_A (16)
#define FRICTION_B (10)
#define I_MAX (200.0f)
#define PID_SPEED_MAX (200)
#define PID_ANGLE_MAX (200)

extern TIM_HandleTypeDef htim2;
extern float angle_setpoint, speed_max;
extern float angle_p, angle_i, angle_d, speed_p, speed_i, speed_d, turn_p;
extern float motor_a_speed, motor_b_speed;
extern float Pitch, Roll, Yaw;
extern float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
extern Status_t status;
extern uint8_t compass_ready;
extern float Roll_I;
extern float motor_a_speed_d, motor_b_speed_d;
extern float yaw_setpoint;

void Control_Init() {
  angle_p = 59.0f; /* 床上能直立的PID 20 0 2.8 */
  angle_d = 17.0f; /* 地上能直立的PID 32 9 1.7 */
  angle_i = 4.2f;
  speed_p = 25.0f;
  speed_i = 0.0f;
  speed_d = 0.0f;
  turn_p = 2.0f;
  speed_max = 400.0f;
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
  HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_RESET);
  while (!compass_ready) osDelay(10);
}

int32_t tmp_a, tmp_b;

void Control_Apply() {
  float tmp_a_s, tmp_b_s, tmp_a_t, tmp_b_t;

  if (STATUS_WORK == status) {
    HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_SET);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
    Roll_I += Roll - angle_setpoint;
    if (Roll_I > I_MAX) Roll_I = I_MAX;
    if (Roll_I < -I_MAX) Roll_I = -I_MAX;

    /* 直立环 */
    tmp_a = tmp_b =
        (Roll - angle_setpoint) * angle_p + Roll_I * angle_i + gyro_x * angle_d;

    /* 速度环 */
    tmp_a_s = tmp_b_s = (motor_a_speed + motor_b_speed) * speed_p +
                        (motor_a_speed_d + motor_b_speed_d) * speed_d;

    /* 转向环 */
    int tmp_yaw = (yaw_setpoint - 180.0f - Yaw);
    if (tmp_yaw > 180) tmp_yaw = 360 - tmp_yaw;
    if (tmp_yaw < -180) tmp_yaw = 360 + tmp_yaw;
    tmp_a_t = -(tmp_b_t = tmp_yaw * turn_p);

    if (tmp_a_s > PID_SPEED_MAX) tmp_a_s = PID_SPEED_MAX;
    if (tmp_b_s > PID_SPEED_MAX) tmp_b_s = PID_SPEED_MAX;
    if (tmp_a_t > PID_ANGLE_MAX) tmp_a_t = PID_ANGLE_MAX;
    if (tmp_b_t > PID_ANGLE_MAX) tmp_b_t = PID_ANGLE_MAX;
    if (tmp_a_s < -PID_SPEED_MAX) tmp_a_s = -PID_SPEED_MAX;
    if (tmp_b_s < -PID_SPEED_MAX) tmp_b_s = -PID_SPEED_MAX;
    if (tmp_a_t < -PID_ANGLE_MAX) tmp_a_t = -PID_ANGLE_MAX;
    if (tmp_b_t < -PID_ANGLE_MAX) tmp_b_t = -PID_ANGLE_MAX;

    tmp_a += (tmp_a_s + tmp_a_t);
    tmp_b += (tmp_b_s + tmp_b_t);

    if (tmp_a == 0) {
      tmp_a = 0;
      HAL_GPIO_WritePin(MOTOR_A_1_GPIO_Port, MOTOR_A_1_Pin, GPIO_PIN_SET);
      HAL_GPIO_WritePin(MOTOR_A_2_GPIO_Port, MOTOR_A_2_Pin, GPIO_PIN_SET);
    } else if (tmp_a < 0) {
      tmp_a = -tmp_a;
      HAL_GPIO_WritePin(MOTOR_A_1_GPIO_Port, MOTOR_A_1_Pin, GPIO_PIN_SET);
      HAL_GPIO_WritePin(MOTOR_A_2_GPIO_Port, MOTOR_A_2_Pin, GPIO_PIN_RESET);
    } else {
      tmp_a = tmp_a;
      HAL_GPIO_WritePin(MOTOR_A_1_GPIO_Port, MOTOR_A_1_Pin, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(MOTOR_A_2_GPIO_Port, MOTOR_A_2_Pin, GPIO_PIN_SET);
    }
    if (tmp_b == 0) {
      tmp_b = 0;
      HAL_GPIO_WritePin(MOTOR_B_1_GPIO_Port, MOTOR_B_1_Pin, GPIO_PIN_SET);
      HAL_GPIO_WritePin(MOTOR_B_2_GPIO_Port, MOTOR_B_2_Pin, GPIO_PIN_SET);
    } else if (tmp_b > 0) {
      tmp_b = tmp_b;
      HAL_GPIO_WritePin(MOTOR_B_1_GPIO_Port, MOTOR_B_1_Pin, GPIO_PIN_SET);
      HAL_GPIO_WritePin(MOTOR_B_2_GPIO_Port, MOTOR_B_2_Pin, GPIO_PIN_RESET);
    } else {
      tmp_b = -tmp_b;
      HAL_GPIO_WritePin(MOTOR_B_1_GPIO_Port, MOTOR_B_1_Pin, GPIO_PIN_RESET);
      HAL_GPIO_WritePin(MOTOR_B_2_GPIO_Port, MOTOR_B_2_Pin, GPIO_PIN_SET);
    }

    if (tmp_a > speed_max) tmp_a = speed_max;
    if (tmp_a <= 0)
      tmp_a = 0;
    else
      tmp_a += FRICTION_A;
    if (tmp_b > speed_max) tmp_b = speed_max;
    if (tmp_b <= 0)
      tmp_b = 0;
    else
      tmp_b += FRICTION_B;

    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, tmp_a);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, tmp_b);
  } else {
    HAL_GPIO_WritePin(STBY_GPIO_Port, STBY_Pin, GPIO_PIN_RESET);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
  }
}
